
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;

import lejos.nxt.*;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.robotics.navigation.*;




/**
 *Example of a navigating robot  operating under remote control
 * uses Command  enum  to decode incoming messages
 * @author Roger Glassey
 */
public class RCNavigator4
{
	static LightSensor sensor = new LightSensor(SensorPort.S1);


	class DataThread implements Runnable {
		//		private DataOutputStream threadDataOut;
		//		public DataThread(DataOutputStream dataOut) {
		//			this.threadDataOut = dataOut;
		//		}
		public void run() {
			while(true){
				LCD.clear();
				try {
					dataOut.writeInt(SensorPort.S1.readRawValue());
					dataOut.writeInt(ultraDistance);
					dataOut.writeInt(sonic.getDistance());
					dataOut.flush();
				} catch (IOException e) {

				}
				try {
					Thread.sleep(20);
				} catch (InterruptedException e) {
				}


			}

		}

	}
	public RCNavigator4(SimpleNavigator aNavigator)
	{
		navigator = aNavigator;

	}
	/**
	 * wheel diameter and track width in cm.
	 * @param args
	 */
	public static void main(String[] args)
	{
		Pilot p = new TachoPilot(2.184f, 4.233f, Motor.A, Motor.C);
		SimpleNavigator nav = new SimpleNavigator(p);
		new RCNavigator4(nav).go();
	}
	/**
	 * decode incoming messages and issue commands to the SimpleNavigator
	 * @param sonic 
	 * @param light 
	 */
	private void readData()
	{
		int code;
		try
		{
			code = dataIn.readInt();
			LCD.clear();
			LCD.drawInt(code,0,1);
			Sound.playTone(800 + 100 * code, 200);
			if (code == Command.GOTO.ordinal())// convert enum to int for comparison
			{
				float x = dataIn.readFloat();
				float y = dataIn.readFloat();
				navigator.goTo(x, y);
			} else if (code == Command.TRAVEL.ordinal())
			{
				float distance = dataIn.readFloat();
				LCD.drawString("D "+Math.round(distance),0 ,2);
				navigator.travel(distance);
			} else if (code == Command.ROTATE.ordinal())
			{
				float angle = dataIn.readFloat();
				LCD.drawString("A "+ Math.round(angle),0,2);
				navigator.rotate(angle);
			}
			Sound.pause(100);
		} catch (IOException e)
		{
		}
	}
	/**
	 * report x,y and heading to mission control
	 * @param sonic 
	 * @param light 
	 */
	public void report(LightSensor light, UltrasonicSensor sonic)
	{
		//	  System.out.println("fdsq");
		//      try
		//      {
		////         dataOut.writeFloat(light.readValue());
		////         dataOut.writeFloat(sonic.getDistance());
		////         dataOut.writeFloat(navigator.getAngle());
		////         dataOut.flush();
		////         LCD.drawInt(Math.round(sonic.getDistance()), 4,0,1);
		////         LCD.drawInt(Math.round(navigator.getY()), 4,5,1);
		////         LCD.drawInt(Math.round(navigator.getAngle()), 4,10,1);
		//      } catch (IOException e)
		//      {
		//      }
	}
	/**
	 * Estabish bluetooth connection to mission control
	 */
	public void connect()
	{
		LCD.clear();
		LCD.drawString("Waiting", 0, 0);
		connection = Bluetooth.waitForConnection(); // this method is very patient. 
		LCD.clear();
		LCD.drawString("Connected", 0, 0);
		dataIn = connection.openDataInputStream();
		dataOut = connection.openDataOutputStream();
		Sound.beepSequence();
	}
	/**
	 * connect and wait for orders
	 */
	private void go()
	{
		sensor.setFloodlight(true);
		connect();
		Thread t = new Thread(new DataThread());
		t.start();
		MuurvolgerGo();
	}

	SimpleNavigator navigator;
	BTConnection connection;
	DataInputStream dataIn;
	DataOutputStream dataOut;

	enum Command  // copied from GridNavControl project
	{
		GOTO,TRAVEL,ROTATE;
	}





	int ultraDistance;
	static int distance = 20;
	int difference;
	Pilot p;
	
	int angle = 45;
	SimpleNavigator nav;
	boolean forward;
	int left;
	float side;
	UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S2);;


	class SonicThread implements Runnable {
		

		public void run() {
			while(true){
				int[] distances;
				distances = new int[10];
				// Omwille van onverklaarbare pieken in de meetwaarden van 255
				// wordt een array gebruikt van 10 meetwaarden.
				
				for(int i =0; i<distances.length;i++){
					distances[i] = sonic.getDistance(); 
					try{
						Thread.sleep(2);
					}catch(InterruptedException e){
					}
				}
				
				// Van de bekomen 10 meetwaarden wordt dan de kleinste berekend,
				// gezien die relatief gezien het meest nauwkeurig zal zijn.
				int smallest = distances[0];
				for(int i = 1; i<distances.length;i++){
					if(distances[i]<=smallest && distances[i]>5)
						smallest = distances[i];;
				}
				ultraDistance = smallest;
			}
		}
	}
	
	class Move implements Runnable {
		public void run(){
			while(true){
				// Voorwaarts rijden ("oneindig" lang) indien dit toegelaten wordt
				if(forward)
					p.travel(10000,true);
				// Anders een boog maken in een richting die bepaald wordt door 
				// de kant waar de muur staat, en hoever de muur is.
				else 
					p.steer(left*side*40,angle,true);
				
			}
		}
	}
	


	private void MuurvolgerGo()
	{
		// Alles initialiseren.
		p = new TachoPilot(2.184f, 4.233f, Motor.A, Motor.C,true);
		Thread t = new Thread(new SonicThread());
		Thread m = new Thread(new Move());
		SetWallSide();
		p.setTurnSpeed(40);
		t.start();
		m.start();
		forward = true;
		while (true) wallFollow();

	}

	// De kant waar de muur staat invoeren.
	private void SetWallSide() {
		int id = Button.waitForPress();
		if(id == Button.ID_LEFT){
			Motor.B.rotate(angle);
			left = -1;
		}
		else{
			Motor.B.rotate(-angle);
			left = 1;
		}
	}
	
	// Hier gebeurt de beslissing of de muur te dicht/ver is, of dat we rechtdoor
	// mogen blijven rijden.
	private void wallFollow(){
		try{
			int ultra = ultraDistance;
			int currentDis = (int) (Math.cos(angle)*ultra);
			System.out.println("CurrentDis = " + currentDis);
			if(currentDis < 0.8*distance){
				side = 1;
				// Als de muur echt te dichtbij komt, sneller wegdraaien.
				if(currentDis < .5*distance){
					side = 1.5f;
				}
				forward = false;
			}
			else if(currentDis > 1.2*distance){
				side = -0.8f;
				// Als de muur "verdwijnt" of een scherpe buitenbocht maakt, sneller
				// meedraaien.
				if(currentDis > 2.5*distance){
					side = -1.3f;
				}
				forward = false;
			}
			else{
				forward = true;
			}

		}
		catch(Exception e){
			System.out.println("EXCEPTION" + e.toString());
			try {
				Thread.sleep(2000);
			} catch (InterruptedException e1) {

			}
		}
	}


}

